/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2022-09-16     chn13935838605       the first version
 */

#include "System.h"
void set_motor_direction(motor_t motor,motor_dir_t dir);
void set_motor_speed(motor_t motor,uint16_t v);
motor_dir_t direction1  = MOTOR_F;
motor_dir_t direction2  = MOTOR_F;
motor_dir_t direction3  = MOTOR_F;


/**
  * @brief  电机驱动函数
  * @param  电机PWM
  * @retval
  */
void MotorOutput(int Motor1_Pwm,int Motor2_Pwm,int Motor3_Pwm)
{
        if (Motor1_Pwm >=0)
        {
            set_motor_direction(MOTOR1,MOTOR_F);
        }
        else
        {
            Motor1_Pwm = -Motor1_Pwm;
            set_motor_direction(MOTOR1,MOTOR_B);
        }
        Motor1_Pwm = (Motor1_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor1_Pwm;

        if (Motor2_Pwm >= 0)
        {
            set_motor_direction(MOTOR2,MOTOR_F);
        }
        else
        {
            Motor2_Pwm = -Motor2_Pwm;
            set_motor_direction(MOTOR2,MOTOR_B);
        }
        Motor2_Pwm = (Motor2_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor2_Pwm;

        Motor3_Pwm = (Motor3_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : Motor3_Pwm;

		set_motor_speed(MOTOR1,Motor1_Pwm);       
		set_motor_speed(MOTOR2,Motor2_Pwm);  
		set_motor_speed(MOTOR3,Motor3_Pwm); 
 }
#if 1
/**
  * @brief  设置电机方向
  * @param  电机，方向
  * @retval
  */
void set_motor_speed(motor_t motor,uint16_t v)
{
  v = (v > PWM_PERIOD_COUNT) ? PWM_PERIOD_COUNT : v;     // 上限处理
  uint32_t dutyfactor = v;
  switch (motor)
  {
  case MOTOR1:
	  if(direction1==MOTOR_F)
		{
			#if defined(io_control)
			SET_MOTOR1_COMPAER(dutyfactor);
			#else
			SET_MOTOR1_COMPAER1(PWM_PERIOD_COUNT);
			SET_MOTOR1_COMPAER2(PWM_PERIOD_COUNT - dutyfactor);
			#endif
		}
	  else
	  {
		  	#if defined(io_control)
			SET_MOTOR1_COMPAER(PWM_PERIOD_COUNT - dutyfactor);
			#else
		    SET_MOTOR1_COMPAER1(PWM_PERIOD_COUNT - dutyfactor);
		    SET_MOTOR1_COMPAER2(PWM_PERIOD_COUNT);
		  	#endif
	  }
	  
      break;
  case MOTOR2:
	  if(direction2==MOTOR_F)
		{
			#if defined(io_control)
			SET_MOTOR2_COMPAER(dutyfactor);
			#else
			SET_MOTOR2_COMPAER1(PWM_PERIOD_COUNT);
			SET_MOTOR2_COMPAER2(PWM_PERIOD_COUNT - dutyfactor);
			#endif
		}
	  else
	  {
		  	#if defined(io_control)
			SET_MOTOR2_COMPAER(PWM_PERIOD_COUNT - dutyfactor);
			#else
		    SET_MOTOR2_COMPAER1(PWM_PERIOD_COUNT - dutyfactor);
		    SET_MOTOR2_COMPAER2(PWM_PERIOD_COUNT);
		  	#endif
	  }
      break;
  
    case MOTOR3:
	  if(direction3==MOTOR_F)
		{

			SET_MOTOR3_COMPAER(dutyfactor);

		}
      break;


  }
  
}
#endif
/**
  * @brief  设置电机方向
  * @param  电机，方向
  * @retval
  */
void set_motor_direction(motor_t motor,motor_dir_t dir)   
{ 
	switch (motor) {
    case MOTOR1:
			direction1=dir;
#if defined(io_control)
            if (dir==MOTOR_F) {

                MOTOR1_IN_RESET;
            }else {

                MOTOR1_IN_SET;
            }
#endif
        break;
    case MOTOR2:
		     direction2=dir;
#if defined(io_control)
            if (dir==MOTOR_F) {

                MOTOR2_IN_RESET;
            }else {

                MOTOR2_IN_SET;
            }
#endif
        break;
	
  }
}

/**
  * @brief  使能电机
  * @param
  * @retval
  */
void set_motor_enable(void)
{
      MOTOR1_ENABLE();
      MOTOR2_ENABLE();
      MOTOR3_ENABLE();
}

/**
  * @brief  失能电机
  * @param
  * @retval
  */
void set_motor_disable(void)
{
    MOTOR1_DISABLE();
    MOTOR2_DISABLE();
    MOTOR3_DISABLE();
}
